# -*- coding: UTF-8 -*-
import os
import threading
import yaml
import numpy as np
import rospy
import cv2
from sensor_msgs.msg import Image
from sensor_msgs.msg import Imu
from cv_bridge import CvBridge
import module.IMU_Optimize
import module.lowlight_enhancement
import queue
import time

lastUpdate=0
isAlive=True

imuTemplate=[]
IMU=queue.Queue()
IMU_Processed=queue.Queue()

Image=queue.Queue()
Image_Processed=queue.Queue()



def initParameter():
    global  imu_topic,image_topic,imu_pub,image_pub
    fs = open(os.path.join("../config", "parameter.yaml"), encoding="UTF-8")
    datas = yaml.load(fs, Loader=yaml.FullLoader)
    imu_topic=datas['imu_topic']
    image_topic=datas["image_topic"]
    imu_pub=datas["imu_pub"]
    image_pub=datas["image_pub"]



def handleIMU(data):
    IMU.put(data)

def handleImage(data):
    a=1
    #print("1")

def dataReplace(list,template):
    a=2
    #print("1")

def consumer():
    while True:
        if IMU_Processed.qsize()>0:
            item=IMU_Processed.get()
            imupub.publish(item)
            imurate.sleep()
        else:
            if isAlive:
                time.sleep(1)
            else:
                print("Producer stopped , end this thread too")
                return


def producer():
    global imuTemplate,lastUpdate,isAlive
    while True:
        list=[]
        current=time.perf_counter()
        Max= 300 if IMU.qsize()>300 else IMU.qsize()
        if Max!=300 and lastUpdate!=0 and current-lastUpdate<9:
            print("Not enough IMU,waiting for new IMU")
            time.sleep(3)
            continue
        elif Max!=300 and lastUpdate!=0 and current-lastUpdate>=9:
            print("too long for new data")
            print(f"IMU size:{IMU.qsize()}")
            if IMU.qsize()==0:
                print("No IMU received!End this thread!")
                isAlive=False
                return
        for i in range(0,Max):
            list.append(IMU.get())
        list.reverse()
        if len(imuTemplate)==0:
            imuTemplate=list.copy()
            while len(list)>0:
                IMU_Processed.put(list.pop())
        else:
            if abs(len(list)-len(imuTemplate))>100:
                imuTemplate=[]
                while len(list)>0:
                    IMU_Processed.put(list.pop())
                lastUpdate = time.perf_counter()
                continue
            score=module.IMU_Optimize.estimate_dtw(list,imuTemplate)
            if score>0.7:
                print("DTW Not matching!")
                dataReplace(list,imuTemplate)
            else:
                while len(list)>0:
                    IMU_Processed.put(list.pop())
        lastUpdate=time.perf_counter()

def process():
    processIMU=threading.Thread(target=producer)
    publishIMU=threading.Thread(target=consumer)
    processIMU.start()
    time.sleep(12)
    publishIMU.start()



if __name__ == "__main__":

    rospy.init_node('FeatureProject',anonymous=True)
    initParameter()
    imusub = rospy.Subscriber(imu_topic, Imu,handleIMU, queue_size=200)
    imgsub=rospy.Subscriber(image_topic,Image,handleImage,queue_size=20)
    imupub=rospy.Publisher(imu_pub,Imu,queue_size=20)
    imgpub=rospy.Publisher(image_pub,Image,queue_size=20)

    imurate = rospy.Rate(200)
    imgrate = rospy.Rate(20)

    featureProject=threading.Thread(target=process)
    featureProject.start()
    rospy.spin()
